package com.HardWare;

import com.friendlyarm.AndroidSDK.HardwareControler;

/**
 * Created by Ziji on 2017/9/16.
 */


public class QMC5883 {
    public static final double MAGNETIC = 1.3;

    public static int QMC5883_Init(int devFD) {
        HardwareControler.setI2CSlave(devFD, (byte) (0x0D));//设置I2c通信硬件地址
        if (HardwareControler.I2CWriteByteData(devFD, (byte) 0x09, (byte) 0x0d, 0) >= 0) {
            return 0;
        }
        return -1;
    }

    static short[] readQMC5883_X(int devFD) {
        byte[] data;
        data = new byte[6];
        for (int i = 0; i < 6; i++) {
            data[i] = (byte) HardwareControler.I2CReadByteData(devFD, 0x00 + i, 0);
        }
        short[] data_16;
        data_16 = new short[3];
        for (int i = 0; i < 3; i++) {
            data_16[i] = change_8to16(data[2 * i+1], data[2 * i]);
        }
        return data_16;
    }

    static short change_8to16(byte x, byte y) {
        short m;
        m= (short) (x>0?x*256+(y>=0?y:256+y):-((256+x)*256+(y>=0?y:256+y)));
        if (m == -4096) {
            m = -2048;
        }
        return m;
    }

    public static double[] get_magnetic(int devFD) {
        short[] data;
        data = readQMC5883_X(devFD);
        double[] data_magnetic;
        data_magnetic = new double[3];
        for (int i = 0; i < 3; i++) {
            data_magnetic[i] = data[i];
            data_magnetic[i]/=120.0;
        }
        return data_magnetic;
    }
}

